//
//  LPFComputator.h
//  LowPassFilter_Standalone
//
//  Created by kadlubek47 on xx.01.2013.
//  Copyright (c) 2013 kadlubek47. All rights reserved.
//

#include "LPFComputator.h"
#include "OpenCLConvolutioner.h"

/*!
 in constructor there is call function setParameters -
 created object always represent some low-pass filter with specific parameters
 */
LPFComputator::LPFComputator(int samplingRate, int fpass, int fstop, float_type oscillationInPass, float_type oscillationInStop, convolutionMethodType convolutionMethod, envokingOpenCLMethodType envokingOpenCLMethod) {
    
    
    openclConvolutioner_ = new OpenCLConvolutioner();
    
    prepareBesselAprox();
    setParameters(samplingRate, fpass, fstop, oscillationInPass, oscillationInStop, convolutionMethod,true);
}

/*!
 h_ is allocated dynamically so it must be released;
 */
LPFComputator::~LPFComputator(void){
    if(NULL!=h_) delete [] h_;
    delete openclConvolutioner_;
}



//! sets filter parameters and calculate h(n)
/*!
 Function is used to set filter parameters.
 After assignments h(n) is calculated.
 
 Function does not check if provided arguments are proper;
 */
void LPFComputator::setParameters( int samplingRate, int fpass, int fstop, float_type oscillationInPass, float_type oscillationInStop, convolutionMethodType convolutionMethod, bool first ) {
    
    convolutionMethod_ = convolutionMethod; //< always update way of convolution
    
    //check if value of parameters really changed
    if(samplingRate_==samplingRate && fpass_==fpass && fstop_==fstop && oscillationInPass_==oscillationInPass && oscillationInStop_==oscillationInStop && !first)
        return;
    
    //1. Assignments
    //1.1. ---
    samplingRate_=samplingRate;
    fpass_=fpass;
    fstop_=fstop;
    oscillationInPass_=oscillationInPass;
    oscillationInStop_=oscillationInStop;
    
    //1.2. normalizedPulsation, normalizedFrequency;
    normalizedFrequency_=((float_type)(fpass_+fstop_))/2/samplingRate;
    normalizedPulsation_=2*((float_type)M_PI)*normalizedFrequency_;
    
    //2. computation of h(n)
    //2.1. A
    float_type minOscill;
    oscillationInPass_ < oscillationInStop_ ? minOscill=oscillationInPass_ : minOscill=oscillationInStop_;
    float_type A=-20*log10(minOscill);
    
    //2.2. Beta
    float_type Beta;
    if(A<21) Beta=0;
    else {
        if(21<=A && A<=50) Beta=0.5842*pow(A-21, (float_type)0.4)+0.07886*(A-21);
        else Beta=0.1102*(A-8.7);
    }
    
    //2.3. N (length of filter)
    int N = (int)ceil ( (A-7.95)* samplingRate_/14.36/(fstop_-fpass_) );
    if(N%2==0) ++N;
    int M = (N-1)/2;
    M_=M;
    //allocation space for filter
    if(NULL==h_) delete [] h_;
    h_=new float_type[N];
    N_=N;
    
    //2.4. Kaiser window coefficients
    //2.4.1 I_O(Beta);	I_0 ->  is the zeroth order Modified Bessel function of the first kind
    float_type I_0_fromBetta=I_0(Beta);
    //2.4.2 Kaiser window coefficients from -M to M (2M+1==N)
    for(int n=-M; n<M+1; ++n) {
        
        //sqrt( 1-pow(((long double)(n-1)-M),2)/pow((long double)M,2))
        h_[n+M]=( I_0( Beta*( sqrt(1-pow(((float_type)n)/(float_type)M, 2)))   ) / I_0_fromBetta );
    }
    
    //2.5 multiply kaiser window coefficients with ideal low-pass filter
    for(int n=0; n<M_; ++n) {
        h_[n]*=idealLowPassFilter(n-M_);
        h_[2*M_-n]*=idealLowPassFilter(n-M_);
    }
    h_[M_]*=idealLowPassFilter(0);
    
    //ok impulseResponse[n] 0 <= n && n <= N-1
}

//! process blocks for each channel
/*
 /param outputBlock ptr to already alocated array
 */
void LPFComputator::process(float_type** inputBlock, float_type** outputBlock /* ptr to already alocated array */, int numOfSamples, int numOfChannels) {
    
    if(channelProcessingUnits_.size() != numOfChannels) {
        
        channelProcessingUnits_.clear();
        
        for(int i=0; i < numOfChannels; ++i) {
            ChannelProcessingUnit one(*this);
            channelProcessingUnits_.push_back(one);
        }
    }
    
    for(int i=0; i < channelProcessingUnits_.size(); ++i)
        channelProcessingUnits_[i].process(inputBlock[i], numOfSamples, outputBlock[i]);
}

/*
 to reset internal buffers containing samples from past call of function process
 */
void LPFComputator::reset() {
    for(int i=0; i < channelProcessingUnits_.size(); ++i)
        channelProcessingUnits_[i].reset();
}



int LPFComputator::getSizeOfImpulseResponse() {
    return N_;
}

//! copy LowPassFilter::h_ to destination
/*!
 /param destination ptr to already alocated array, to alocate appriorate size of memory at first use getSizeOfImpulseResponse
 */
void LPFComputator::getImpulseResponse(float_type* destination) {
    memcpy(destination, h_, N_*sizeof(float_type));
    return;
}



/*!
 equation of ideal low pass filter;
 normalized pulsation is needed;
 */
float_type LPFComputator::idealLowPassFilter(int sampleNum) {
    if(sampleNum==0) return 2*normalizedFrequency_;
    return 2*normalizedFrequency_*sin(normalizedPulsation_*sampleNum)/(normalizedPulsation_*sampleNum);
}

/*!
 compute auxiliary table for I_O(float_type) function;
 Expr = 2^k * k!; result will be put in besselElem table;
 */
void LPFComputator::prepareBesselAprox() {
    besselElem[0]=1;
    for(int i=1; i<numElemApprox; ++i) {
        besselElem[i]=besselElem[i-1]*2*i;
    }
}

//! zeroth order Modified Bessel function of the first kind
/*!
 aproximation made by >>numElemApprox<< first elements sumed
 computation done on longer_float_type
 */
float_type LPFComputator::I_0(longer_float_type x) {
    longer_float_type besselSum=0;
    for(int i=1; i<numElemApprox; ++i) {
        besselSum+=pow(pow(x, i)/besselElem[i],2);
    }
    return besselSum+1;
}



inline LPFComputator::ChannelProcessingUnit::ChannelProcessingUnit(LPFComputator& base): base_(&base) {}

#pragma mark ProcessOneChannel
void LPFComputator::ChannelProcessingUnit::process(float_type* inputBlock, int inputBlockSize, float_type* outputBlock) {
    
    signal_.setParameter(base_->M_, inputBlockSize);
    signal_.update(inputBlock);
    
    switch (base_->convolutionMethod_) {
            
        case NORMAL_CPU:
            convoluteWithImpulseResponse(signal_.signal_, signal_.signalLength_, outputBlock);
            break;
            
        //case OPENCL_CPU:
        case OPENCL_GPU:
            base_->openclConvolutioner_->process(signal_.signal_/*x*/, signal_.signalLength_/*x_size*/, base_->h_, base_->M_, outputBlock/*y*/);
            break;
            
        default:
            convoluteWithImpulseResponse(signal_.signal_, signal_.signalLength_, outputBlock);
            break;
    }
    
}

void LPFComputator::ChannelProcessingUnit::reset() {
    signal_.clear();
}

/*!
 convolution of x(n) and h(n)
 
 |block of size M_| | samples for which result samples are computed | |block of size M_|: x(i+M_) -> y(i), 0 <= i <= sizeX-2*M
 stepping down and up are not get into considerations
 
 /param y >>out<<, ptr to already alocated array
 */
void LPFComputator::ChannelProcessingUnit::convoluteWithImpulseResponse (float_type* x, int sizeX, float_type* y/*out, ptr to already alocated array*/) {
    
    for(int index_y=0; index_y<sizeX-2* base_->M_; ++index_y) {
        y[index_y]=0;
        int index_h=0;
        int index_x=index_y;
        
        for (; index_h < base_->N_; ++index_h, ++index_x)
            y[index_y] += x[index_x] * base_->h_[index_h];
        
    }
    
}



inline LPFComputator::ChannelProcessingUnit::SignalWindow::SignalWindow() {}

inline LPFComputator::ChannelProcessingUnit::SignalWindow::~SignalWindow() { delete [] signal_; }

void LPFComputator::ChannelProcessingUnit::SignalWindow::setParameter(unsigned int M, unsigned int x_size) {
    if( x_size==0)
        return;
    
    if( M + x_size != M_+x_size_){
        delete [] signal_;
        
        signalLength_ = 2*M + x_size;
        signal_ = new float_type[signalLength_]();
    }
    
    M_ = M;
    x_size_ = x_size;
}

void LPFComputator::ChannelProcessingUnit::SignalWindow::update(float_type* x) {
    unsigned int i;
    for(i=0; i < 2*M_; ++i)
        signal_[i] = signal_[signalLength_-2*M_+i];
    for(int j=0; j < x_size_; ++j, ++i)
        signal_[i] = x[j];
}

void LPFComputator::ChannelProcessingUnit::SignalWindow::clear() {
    for(int i=0; i < signalLength_; ++i)
        signal_[i] = 0;
}

